    .cpu    cortex-m3
    .fpu    softvfp
    .syntax unified
    .thumb
    .text

    .equ    SCB_VTOR, 0xE000ED08            /* Vector Table Offset Register */
    .equ    ICSR, 0xE000ED04                /* interrupt control state register */
    .equ    PENDSVSET_BIT, 0x10000000       /* value to trigger PendSV exception */

    .equ    SHPR3, 0xE000ED20               /* system priority register (3) */
    .equ    PENDSV_PRI_LOWEST, 0xFFFF0000   /* PendSV and SysTick priority value (lowest) */

    .global interrupt_disable
    .type interrupt_disable, %function
interrupt_disable:
    mrs     r0, primask
    cpsid   i
    bx      lr

    .global interrupt_enable
    .type interrupt_enable, %function
interrupt_enable:
    msr     primask, r0
    bx      lr

    .global context_switch
    .type context_switch, %function
context_switch:
    ldr     r2, =thread_switch_flag
    ldr     r3, [r2]
    cmp     r3, #1
    beq     _reswitch
    mov     r3, #1
    str     r3, [r2]

    ldr     r2, =thread_from
    str     r0, [r2]

_reswitch:
    ldr     r2, =thread_to
    str     r1, [r2]

    ldr     r0, =ICSR
    ldr     r1, =PENDSVSET_BIT
    str     r1, [r0]
    bx      lr

    .global context_switch_to
    .type context_switch_to, %function
context_switch_to:
    ldr     r1, =thread_to
    str     r0, [r1]

    ldr     r1, =thread_switch_flag
    mov     r0, #1
    str     r0, [r1]

    /* set the PendSV and SysTick exception priority */
    ldr     r0, =SHPR3
    ldr     r1, =PENDSV_PRI_LOWEST
    ldr.w   r2, [R0,#0]             /* read */
    orr     r1, r1, r2              /* modify */
    str     r1, [r0]                /* write-back */

    ldr     r0, =ICSR
    ldr     r1, =PENDSVSET_BIT
    str     r1, [r0]

    /* restore MSP */
    ldr     r0, =SCB_VTOR
    ldr     r0, [r0]
    ldr     r0, [r0]
    nop
    msr     msp, r0

    /* enable interrupts at processor level */
    cpsie   f
    cpsie   i

    .global PendSV_Handler
    .type PendSV_Handler, %function
PendSV_Handler:
    mrs     r2, primask
    cpsid   i

    ldr     r0, =thread_switch_flag
    ldr     r1, [r0]
    cbz     r1, pendsv_exit

    mov     r1, #0
    str     r1, [r0]

    ldr     r0, =thread_from
    ldr     r1, [r0]
    cbz     r1, switch_to_thread

    mrs     r1, psp
    stmfd   r1!, {r4 - r11}
    ldr     r0, [r0]
    str     r1, [r0]

switch_to_thread:
    ldr     r1, =thread_to
    ldr     r1, [r1]
    ldr     r1, [r1]

    ldmfd   r1!, {r4 - r11}
    msr     psp, r1

pendsv_exit:
    msr     primask, r2

    orr     lr, lr, #0x04
    bx      lr

    .global HardFault_Handler
    .type   HardFault_Handler, %function
HardFault_Handler:
    mrs     r0, msp
    tst     lr, #0x04
    beq     get_sp_done
    mrs     r0, psp

get_sp_done:
    stmfd   r0!, {r4 - r11}
    stmfd   r0!, {lr}

    tst     lr, #0x04
    beq     update_msp
    msr     psp, r0
    b       update_done

update_msp:
    msr     msp, r0

update_done:
    push    {lr}
    bl      hard_fault_handler
    pop     {lr}

    orr     lr, lr, #0x04
    bx      lr

